/**
  ******************************************************************************
  * @file    RPLIDAR.c
  * @author  Ah_Da 
  * @brief   USART2->RPlidar communiocation BAUDRATE 115200
	*					 TIM2 CHANNEL 1 Generate PWM for lidar's wheel
	*					 PWM FREQUENCY 25K DUTYCYCLE 60%  LIDAR SPINFREQUENCY 10 RPM
	*
	* @version 1    
  ******************************************************************************
  */
	
#include <math.h>
#include "rplidar.h"
#include <stdio.h>
#include <stdarg.h>
#include <stdlib.h>
#include "cmsis_os.h"
/*****************************************************************************/
#define FREQ 2879
#define motor_dutyCycle FREQ*0.7
#define PI 3.141592

float sinNum[91]={0.000000,0.017452,0.034899,0.052336,0.069756,0.087156,0.104528,0.121869,0.139173,0.156434,0.173648,0.190809,0.207912,
	0.224951,0.241922,0.258819,0.275637,0.292372,0.309017,0.325568,0.342020,0.358368,0.374607,0.390731,0.406737,0.422618,0.438371,
	0.453991,0.469472,0.484810,0.500000,0.515038,0.529919,0.544639,0.559193,0.573576,0.587785,0.601815,0.615661,0.629320,0.642788,
	0.656059,0.669131,0.681998,0.694658,0.707107,0.719340,0.731354,0.743145,0.754710,0.766044,0.777146,0.788011,0.798636,0.809017,
	0.819152,0.829038,0.838671,0.848048,0.857167,0.866025,0.874620,0.882948,0.891007,0.898794,0.906308,0.913545,0.920505,0.927184,
	0.933580,0.939693,0.945519,0.951057,0.956305,0.961262,0.965926,0.970296,0.974370,0.978148,0.981627,0.984808,0.987688,0.990268,
	0.992546,0.994522,0.996195,0.997564,0.998630,0.999391,0.999848,1.000000};
float cosNum[91]={1.000000,0.999848,0.999391,0.998630,0.997564,0.996195,0.994522,0.992546,0.990268,0.987688,0.984808,0.981627,0.978148,
	0.974370,0.970296,0.965926,0.961262,0.956305,0.951057,0.945519,0.939693,0.933580,0.927184,0.920505,0.913545,0.906308,0.898794,0.891007,
	0.882948,0.874620,0.866025,0.857167,0.848048,0.838671,0.829038,0.819152,0.809017,0.798635,0.788011,0.777146,0.766044,0.754710,0.743145,
	0.731354,0.719340,0.707107,0.694658,0.681998,0.669131,0.656059,0.642788,0.629320,0.615661,0.601815,0.587785,0.573576,0.559193,0.544639,
	0.529919,0.515038,0.500000,0.484810,0.469472,0.453990,0.438371,0.422618,0.406737,0.390731,0.374607,0.358368,0.342020,0.325568,0.309017,
	0.292372,0.275637,0.258819,0.241922,0.224951,0.207912,0.190809,0.173648,0.156434,0.139173,0.121869,0.104528,0.087156,0.069757,0.052336,
	0.034899,0.017452,-0.000000};

extern TIM_HandleTypeDef htim2;
extern UART_HandleTypeDef huart1;
extern UART_HandleTypeDef huart2;
extern UART_HandleTypeDef huart3;



char EAI_Start_Byte=0xa5;
char EAI_Start_Scan=0x20;
char EAI_Stop_Scan=0x25;
char EAI_ForceStart=0x21;
char EAI_GetHealth=0x52;
char EAI_Reset=0x40;

unsigned char  Rx_data[3];
unsigned char  Rx_Buffer[3][1200], USART_Transfer_cplt[3]={0,0,0};
uint32_t Rx_indx[3]={0,0,0};
uint16_t obstacle[5][361];//360 degree the resolution for RPLIDAR is 1 degree
uint16_t obstacle_distance[361];
uint8_t num=0;
uint8_t Lidar_receive_time[3]={10,10,10};
uint8_t flag1=1;
uint8_t ans_data[3][7];
uint8_t scan_data[3][5];
uint8_t data_index[3]={0,0,0};
uint8_t data_scan_flag[3]={0,0,0};
void Start_Receive_lidardata(uint8_t uart_order)
{

	switch(uart_order)
	{
		case 1:
				 HAL_UART_Receive_DMA(&huart1, &Rx_data[0], 1);
		break;
		case 2:
				 HAL_UART_Receive_DMA(&huart2, &Rx_data[1], 1);
	  break;
		case 3:
				HAL_UART_Receive_DMA(&huart3, &Rx_data[2], 1);
		break;
		default:
			break;//error
		
	}
	

}
uint8_t RPLIDAR_ROTATION_START(void)
{

	 HAL_TIM_PWM_Start(&htim2,TIM_CHANNEL_1);
   __HAL_TIM_SetCompare(&htim2, TIM_CHANNEL_1, motor_dutyCycle);	
   return 1;
}

uint8_t RPLIDAR_SCAN_START(uint8_t uart_order,Lidar_Type *lidar)
{
	 for(uint8_t i=0;i<LIDAR_LOOP_NUM;i++)
	 for(uint16_t m=0;m<LIDAR_DEGREE;m++)
			lidar->obstacle[i][m]=0x0000;
	
	lidar->numOfCollection=0;
	switch(uart_order)
	{
		case 1:
				HAL_UART_Transmit(&huart1, (uint8_t *)&EAI_Start_Byte, 1,10);	
		    HAL_UART_Transmit(&huart1, (uint8_t *)&EAI_Start_Scan, 1,10);	
		break;
		case 2:
				HAL_UART_Transmit(&huart2, (uint8_t *)&EAI_Start_Byte, 1,10);	
		    HAL_UART_Transmit(&huart2, (uint8_t *)&EAI_Start_Scan, 1,10);	
		break;
		case 3:
				HAL_UART_Transmit(&huart3, (uint8_t *)&EAI_Start_Byte, 1,10);	
		    HAL_UART_Transmit(&huart3, (uint8_t *)&EAI_Start_Scan, 1,10);	
		break;
		default:
			break;
			
	}
	
//	 printf("%c",EAI_Start_Byte);
//	 printf("%c",EAI_Start_Scan);	
   return 1;
}
uint8_t RPLIDAR_SCAN_Stop(uint8_t uart_order,Lidar_Type *lidar)
{

	switch(uart_order)
	{
		case 1:
				HAL_UART_Transmit(&huart1, (uint8_t *)&EAI_Start_Byte, 1,10);	
		    HAL_UART_Transmit(&huart1, (uint8_t *)&EAI_Stop_Scan, 1,10);	
		break;
		case 2:
				HAL_UART_Transmit(&huart2, (uint8_t *)&EAI_Start_Byte, 1,10);	
		    HAL_UART_Transmit(&huart2, (uint8_t *)&EAI_Stop_Scan, 1,10);	
		break;
		case 3:
				HAL_UART_Transmit(&huart3, (uint8_t *)&EAI_Start_Byte, 1,10);	
		    HAL_UART_Transmit(&huart3, (uint8_t *)&EAI_Stop_Scan, 1,10);	
		break;
		default:
		break;
			
	}
	osDelay(100);
	
		 for(uint8_t i=0;i<LIDAR_LOOP_NUM;i++)
	 for(uint16_t m=0;m<LIDAR_DEGREE;m++)
			lidar->obstacle[i][m]=0x0000;
	
	lidar->numOfCollection=0;
	
//	 printf("%c",EAI_Start_Byte);
//	 printf("%c",EAI_Start_Scan);	
   return 1;
}
uint8_t RPLIDAR_RESTARTSCAN(uint8_t uart_order,Lidar_Type *lidar)
{
	  uint8_t array_num=uart_order-1;
    RPLIDAR_SCAN_Stop(uart_order,lidar);
	//  RPLIDAR_ROTATION_START();
	  osDelay(500);
		data_scan_flag[array_num]=0;
		for (int i=0; i<Rx_indx[array_num]; i++)
		{
					Rx_Buffer[array_num][i] = 0x00;
		}
		for(uint8_t i=0;i<7;i++)
		 ans_data[array_num][i]=0x00;
		Rx_indx[array_num]=0;
	 for(uint8_t i=0;i<5;i++)
	   for(uint16_t m=0;m<361;m++)
		    lidar->obstacle[i][m]=0x0000;
		
		data_scan_flag[array_num]=0;
		USART_Transfer_cplt[array_num]=0;
		flag1=1;
		RPLIDAR_SCAN_START(uart_order,lidar);

//	  printf("%c",EAI_Start_Byte);
//	  printf("%c",EAI_ForceStart );	
    return 1;
}
uint8_t isReceiveOvertime(uint8_t uart_order)
{
	uint8_t array_num=uart_order-1;
	
	if(data_scan_flag[array_num]==1)
		if(Lidar_receive_time[array_num]==0)
			 return 1;
		
	return 0;


}

uint8_t collect_LIDARdata(uint8_t uart_order,Lidar_Type *lidar)
{
	 uint8_t array_num=uart_order-1;
	
		if(USART_Transfer_cplt[array_num])
		{
			for(uint16_t i=0;i<Rx_indx[array_num];i+=5)
			{
				uint16_t angle=(uint16_t)(Rx_Buffer[array_num][i+2]<<1)+((float)Rx_Buffer[array_num][i+1]/128+0.55);
				uint16_t distance=(uint16_t)(Rx_Buffer[array_num][i+4]<<6)+(Rx_Buffer[array_num][i+3]>>2);
					lidar->obstacle[lidar->numOfCollection][angle]=distance;			
			}
			lidar->numOfCollection++;
			if(lidar->numOfCollection==5)
			{
				for(uint8_t i=0;i<(LIDAR_LOOP_NUM-1);i++)
				for(uint16_t m=0;m<361;m++)
				{
					lidar->obstacle[i][m]=0x0000;
					lidar->obstacle[i][m]=lidar->obstacle[i+1][m];
				
				}
				
				for(uint16_t m=0;m<361;m++)
					lidar->obstacle[LIDAR_LOOP_NUM-1][m]=0x0000;
				lidar->numOfCollection--;
			}
			
			for (int i=0; i<Rx_indx[array_num]; i++)
			{
					Rx_Buffer[array_num][i] = 0x00;
			}
			Rx_indx[array_num]=0;
		  USART_Transfer_cplt[array_num]=0;
				return 1;
		}
	return 0;

}

//0 for NO obstavle;1 for in the emergrncy stop zone ;2 for in the low speed zone
uint8_t Detect_Obstacles(Lidar_Type *lidar,int16_t Min_X,int16_t Max_X,int16_t Min_Y,int16_t Max_Y,uint16_t Detect_Angle)
{
	uint8_t obstacle_parameter=0;
	double x,y;
	uint8_t zone=Detect_Angle/90;
	uint16_t distance=0;
//	double radian_angle;
//	uint8_t effectiveNum=0;
	
//	for(uint8_t m=0;m<4;m++)
//	if(lidar->obstacle[m][Detect_Angle]>0)
//	{
//		distance+=lidar->obstacle[m][Detect_Angle];
//		effectiveNum++;
//	
//	}
//	if(effectiveNum==0)
//		distance=0;
//	else
//		distance/=effectiveNum;
	distance=Get_Distance(lidar,Detect_Angle);
	
	if(distance>0)
	switch (zone)
	{
		case 0:
//			radian_angle=(double)Detect_Angle*PI/180;
			x=distance*sinNum[Detect_Angle];
			y=distance*cosNum[Detect_Angle];
		  if((x>=Min_X)&&(x<=Max_X))
			{
				if(y<=Min_Y)
					obstacle_parameter=1;
				else if(y<Max_Y)
					obstacle_parameter=2;
			}
			else
				obstacle_parameter=0;
			break;
		case 1:
			Detect_Angle=Detect_Angle-90.0;
  		x=distance*cosNum[Detect_Angle];
			y=distance*sinNum[Detect_Angle]*(-1);
			if(y>=Min_Y&&y<=Max_Y)
			{
				if(x<=Min_X)
					obstacle_parameter=1;
				else if(x<Max_X)
					obstacle_parameter=2;
			}
			else
				obstacle_parameter=0;
			break;
		case 2:
			Detect_Angle=Detect_Angle-180.0;
  		x=distance*sinNum[Detect_Angle]*(-1);
			y=distance*cosNum[Detect_Angle]*(-1);
			if(y>=Min_Y&&y<=Max_Y)
			{
				if(fabs(x)<=Min_X)
					obstacle_parameter=1;
				else if(fabs(x)<Max_X)
					obstacle_parameter=2;
			}
			else
				obstacle_parameter=0;
			break;
		case 3:
			Detect_Angle=Detect_Angle-270.0;
			x=distance*cosNum[Detect_Angle]*(-1);
			y=distance*sinNum[Detect_Angle];
			if(x>Min_X&&x<=Max_X)
			{
				if(y<=Min_Y)
					obstacle_parameter=1;
				else if(y<Max_Y)
					obstacle_parameter=2;
			}
			else
				obstacle_parameter=0;
			break;
		default: 
			break;

	}
	
	return obstacle_parameter;

}

int16_t Get_Distance(Lidar_Type *lidar,uint16_t angle)
{
	uint16_t distance=0;
	uint8_t effectiveNum=0;
	for(uint8_t m=0;m<4;m++)
	if(lidar->obstacle[m][angle]>0)
	{
		distance+=lidar->obstacle[m][angle];
		effectiveNum++;
	
	}
	
	if(effectiveNum==0)
		distance=0;
	else
		distance/=effectiveNum;
	
	return distance;

}
void Get_ScanData(uint8_t uart_order)
{
      if(data_scan_flag[0]==1)
      {
				  if(flag1==1)
					{
		        for(uint8_t i=0;i<7;i++)
           {
              if(ans_data[0][i]==0x00)
                  UART2_Printf("%02x",0x00);
              else
                  UART2_Printf("%02x",ans_data[0][i]);        
								
							ans_data[0][i]=0x00;
           }
						flag1=0;
					}
				}


}


